#!/usr/bin/env python
# encoding: utf-8

import rospy
import geometry_msgs.msg
import time

class CartesianControlPosePublisher(object):
    def __init__(self):
        # 初始化节点
        rospy.init_node('cartesian_control_pose_publisher')

        # 定义发送器
        self.pub_ = rospy.Publisher('cartesian_control_topic', geometry_msgs.msg.Pose, queue_size=10)

    def publish_pose(self, pose_msg):
        # 发布姿态消息
        self.pub_.publish(pose_msg)

def main():
    publi = CartesianControlPosePublisher()

    while not rospy.is_shutdown():
        pose_msg = geometry_msgs.msg.Pose()

        # 设置消息的坐标
        pose_msg.position.x = 0.45611993071
        pose_msg.position.y = 0.00195518774071
        pose_msg.position.z = 0.434136790952

        pose_msg.orientation.x = 0.499762278048
        pose_msg.orientation.y = 0.499918626181
        pose_msg.orientation.z = 0.500809414448
        pose_msg.orientation.w = 0.499508721684

        # 发布消息
        publi.publish_pose(pose_msg)

        # 等待一秒
        rospy.sleep(1)

if __name__ == '__main__':
    main()
